/**
 ******************************************************************************
 * @file    bsp_mpu6050.c
 * @brief   MPU6050 六轴姿态传感器驱动模块
 * @details 支持软件 I2C 与硬件 I2C 方式，包含初始化、数据采集与配置接口。
 * @date    2022-8-12
 ******************************************************************************
 */

#include "mpu6050.h"
#include "mpui2c.h"
#include "bsp_delay.h"
#include "usart.h"
#include "i2c.h"

/* ========================================================================== */
/*                            MPU6050 基础配置函数                            */
/* ========================================================================== */

/**
 * @brief  初始化 MPU6050 传感器
 * @retval 0 表示成功，非 0 表示失败
 * @note   包括设备复位、陀螺仪/加速度计配置及采样率设置。
 */
uint8_t MPU_Init(void)
{
#if defined(MPU6050_Software_I2C)
    I2C_GPIO_Init();  /* 软件 I2C 引脚初始化 */
#endif

    MPU_Write_Byte(MPU_PWR_MGMT1_REG, 0x80); /* 复位 MPU6050 */
    HAL_Delay(100);

    MPU_Write_Byte(MPU_PWR_MGMT1_REG, 0x00); /* 唤醒 MPU6050 */
    MPU_Set_Gyro_Fsr(3);                     /* 陀螺仪 ±2000°/s */
    MPU_Set_Accel_Fsr(0);                    /* 加速度计 ±2g */
    MPU_Set_Rate(50);                        /* 采样率 50Hz */

    /* 关闭中断与 FIFO 功能 */
    MPU_Write_Byte(MPU_INT_EN_REG, 0x00);
    MPU_Write_Byte(MPU_USER_CTRL_REG, 0x00);
    MPU_Write_Byte(MPU_FIFO_EN_REG, 0x00);

    /* 配置中断引脚逻辑（低电平有效） */
    MPU_Write_Byte(MPU_INTBP_CFG_REG, 0x80);

    /* 校验设备 ID（可选） */
    MPU_Read_Byte(MPU_DEVICE_ID_REG);

    /* 设置时钟源与工作模式 */
    MPU_Write_Byte(MPU_PWR_MGMT1_REG, 0x01);
    MPU_Write_Byte(MPU_PWR_MGMT2_REG, 0x00);

    MPU_Set_Rate(200);  /* 重新设置采样率为 200Hz */

    return 0;
}

/* ========================================================================== */
/*                              参数配置函数                                  */
/* ========================================================================== */

/**
 * @brief 设置陀螺仪量程
 * @param fsr  0=±250°/s, 1=±500°/s, 2=±1000°/s, 3=±2000°/s
 * @retval 0 成功，非 0 失败
 */
uint8_t MPU_Set_Gyro_Fsr(uint8_t fsr)
{
    return MPU_Write_Byte(MPU_GYRO_CFG_REG, fsr << 3);
}

/**
 * @brief 设置加速度计量程
 * @param fsr  0=±2g, 1=±4g, 2=±8g, 3=±16g
 * @retval 0 成功，非 0 失败
 */
uint8_t MPU_Set_Accel_Fsr(uint8_t fsr)
{
    return MPU_Write_Byte(MPU_ACCEL_CFG_REG, fsr << 3);
}

/**
 * @brief 设置数字低通滤波器（LPF）
 * @param lpf 截止频率（Hz）
 * @retval 0 成功，非 0 失败
 */
uint8_t MPU_Set_LPF(uint16_t lpf)
{
    uint8_t data;

    if (lpf >= 188)      data = 1;
    else if (lpf >= 98)  data = 2;
    else if (lpf >= 42)  data = 3;
    else if (lpf >= 20)  data = 4;
    else if (lpf >= 10)  data = 5;
    else                 data = 6;

    return MPU_Write_Byte(MPU_CFG_REG, data);
}

/**
 * @brief 设置采样率
 * @param rate 采样率 (4~1000Hz)
 * @retval 0 成功，非 0 失败
 * @note   自动将 LPF 设置为采样率一半
 */
uint8_t MPU_Set_Rate(uint16_t rate)
{
    uint8_t data;

    if (rate > 1000) rate = 1000;
    if (rate < 4)    rate = 4;

    data = 1000 / rate - 1;
    MPU_Write_Byte(MPU_SAMPLE_RATE_REG, data);

    return MPU_Set_LPF(rate / 2);
}

/* ========================================================================== */
/*                              数据读取函数                                  */
/* ========================================================================== */

/**
 * @brief  读取芯片温度（单位：0.01°C）
 * @retval 温度值，放大 100 倍
 */
int16_t MPU_Get_Temperature(void)
{
    uint8_t buf[2];
    int16_t raw;
    float temp;

    MPU_Read_Len(MPU_ADDR, MPU_TEMP_OUTH_REG, 2, buf);
    raw = ((uint16_t)buf[0] << 8) | buf[1];
    temp = 36.53f + ((float)raw / 340.0f);

    return (int16_t)(temp * 100);
}

/**
 * @brief  读取陀螺仪三轴原始数据
 * @param  gx,gy,gz 陀螺仪三轴输出指针
 * @retval 0 成功，非 0 失败
 */
uint8_t MPU_Get_Gyroscope(int16_t *gx, int16_t *gy, int16_t *gz)
{
    uint8_t buf[6];
    uint8_t res = MPU_Read_Len(MPU_ADDR, MPU_GYRO_XOUTH_REG, 6, buf);

    if (res == 0)
    {
        *gx = ((int16_t)buf[0] << 8) | buf[1];
        *gy = ((int16_t)buf[2] << 8) | buf[3];
        *gz = ((int16_t)buf[4] << 8) | buf[5];
    }

    return res;
}

/**
 * @brief  读取加速度计三轴原始数据
 * @param  ax,ay,az 加速度三轴输出指针
 * @retval 0 成功，非 0 失败
 */
uint8_t MPU_Get_Accelerometer(int16_t *ax, int16_t *ay, int16_t *az)
{
    uint8_t buf[6];
    uint8_t res = MPU_Read_Len(MPU_ADDR, MPU_ACCEL_XOUTH_REG, 6, buf);

    if (res == 0)
    {
        *ax = ((int16_t)buf[0] << 8) | buf[1];
        *ay = ((int16_t)buf[2] << 8) | buf[3];
        *az = ((int16_t)buf[4] << 8) | buf[5];
    }

    return res;
}
